import serial
import struct


'''
    协议很简单
    主机： <0x80> <目标地址> <发送数据包长度> <发送数据包> <0xff>
    下位机：<0x90> <目标地址> <返回数据包长度> <返回数据包>
    其中地址分配为0x00-0x7f为只写地址，下位机不需要返回
    0x80-0xff为只读地址，下位机必须返回
    只要调用API就行了
'''

class CarControl():
    def __init__(self,serialDevice):
        self.serialObject = serial.Serial(serialDevice,baudrate=115200)
        self.serialObject.read_all()
    def sendPacket(self, address,data):
        packet = b''
        packet += bytes([0x80])
        packet += bytes([address])
        packet += bytes([len(data)])
        packet += data
        packet += bytes([0xff])
        # print(packet)
        self.serialObject.write(packet)

    def recvPacket(self):
        self.serialObject.read_until(b'\x90')
        addr = ord(self.serialObject.read(1))
        length = ord(self.serialObject.read(1))
        if length != 0:
            data = self.serialObject.read(length)
        stop = ord(self.serialObject.read(1))
        if stop != 0xff:
            return None
        return (addr,data)

    def setSpeed(self,speed,dspeed):
        # 暂且设置速度地址为0x01，差速地址为0x02
        # 直接发送整个32位浮点型数据
        speedData = struct.pack('f',speed)
        self.sendPacket(0x00,speedData)
        dspeedData = struct.pack('f',dspeed)
        self.sendPacket(0x01,dspeedData)

    def setMessage(self,line,message):
        assert line < 4 and line >= 0 
        message = str(message)
        messageData = message.encode('utf-8')
        self.sendPacket(0x02+line,messageData)
        
    def getMPU(self):
        self.sendPacket(0x80,b'')
        addr, packet = self.recvPacket()
        if addr != 0x80:
            return None
        yaw,roll,pinch = struct.unpack('fff',packet)
        return (yaw,roll,pinch)
    
    def getSwStatus(self):
        self.sendPacket(0x81,b'')
        addr,packet = self.recvPacket()
        if addr != 0x81:
            return None
        if packet[0] == 0:
            return 0
        else: return 1
